import math


def Sub(p1, p2):
  return p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]


def Add(p1, p2):
  return p1[0] + p2[0], p1[1] + p2[1], p1[2] + p2[2]


def Sub2(p1, p2):
  return p1[0] - p2[0], p1[1] - p2[1]


def Add2(p1, p2):
  return p1[0] + p2[0], p1[1] + p2[1]


def MulNumber(v, n):
  return v[0] * n, v[1] * n, v[2] * n


def Length(v):
  return math.sqrt(v[0] ** 2 + v[1] ** 2 + v[2] ** 2)


def Distance(p1, p2):
  return Length(Sub(p1, p2))


def Rot(v):
  y = v[1]
  xz = math.sqrt(v[0] ** 2 + v[2] ** 2)
  return math.atan2(y, xz), math.atan2(v[2], v[0])


def Unity(v):
  length = Length(v)
  p = 1.0 / length
  return v[0] * p, v[1] * p, v[2] * p


def Round(v, s=2):
  return round(v[0], s), round(v[1], s), round(v[2], s)


def Degrees(v):
  return math.degrees(v[0]), math.degrees(v[1]), math.degrees(v[2])


def RotDegrees(v):
  rot = Rot(v)
  return math.degrees(rot[0]), math.degrees(rot[1])


def RotateX(v, angle):
  """绕X轴旋转"""
  r = math.sqrt(v[1] ** 2 + v[2] ** 2)
  oldAngle = math.atan2(v[1], v[2])
  return v[0], math.sin(oldAngle + angle) * r, math.cos(oldAngle + angle) * r


def RotateY(v, angle):
  """绕Y轴旋转"""
  r = math.sqrt(v[0] ** 2 + v[2] ** 2)
  oldAngle = math.atan2(v[2], v[0])
  return math.cos(oldAngle + angle) * r, v[1], math.sin(oldAngle + angle) * r


def RotateZ(v, angle):
  """绕Z轴旋转"""
  r = math.sqrt(v[0] ** 2 + v[1] ** 2)
  oldAngle = math.atan2(v[1], v[0])
  return math.cos(oldAngle + angle) * r, math.sin(oldAngle + angle) * r, v[2]


def RotToDir(rot):
  xLocalPos = RotateX((0, 0, 1), rot[0])
  """绕x旋转后"""
  xyLocalPos = RotateY(xLocalPos, rot[1])
  """绕y旋转后"""
  xyzLocalPos = RotateZ(xyLocalPos, rot[2])
  """绕z旋转后"""
  return xyzLocalPos


def DirToRot(v):
  zRot = math.atan2(v[1], v[0])
  v = RotateZ(v, -zRot)
  """还原z旋转"""
  yRot = math.atan2(-v[0], v[2])
  v = RotateY(v, -yRot)
  """还原y旋转"""
  xRot = math.atan2(v[1], v[2])
  # v = RotateZ(v, -xRot)
  # """还原x旋转"""
  return xRot, yRot, zRot


def L2W(cRot, cPos, localPos):
  """相机坐标系到世界坐标系；还原z,y,x,平移"""
  localPos = RotateZ(localPos, cRot[2])
  localPos = RotateY(localPos, cRot[1])
  localPos = RotateX(localPos, cRot[0])
  # localPos = Add(localPos, cPos)
  return localPos


def W2L(cRot, cPos, fov, worldPos):
  """
  相机先旋转（默认看向z轴，x旋转，y旋转，z旋转），再设置世界坐标的。
  同理，目标坐标需要逆向相机的旋转：-x旋转，-y旋转，-z旋转
  :param cRot: 相机的世界坐标x, y, z轴旋转，单位是弧度
  :param cPos:
  :param fov:
  :param worldPos:
  :return:
  """
  localPos = Sub(worldPos, cPos)
  """以相机所在位置为原点的世界坐标"""
  xLocalPos = RotateX(localPos, -cRot[0])
  """绕x旋转后"""
  xyLocalPos = RotateY(xLocalPos, -cRot[1])
  """绕y旋转后"""
  xyzLocalPos = RotateZ(xyLocalPos, -cRot[2])
  """绕z旋转后"""
  d = xyzLocalPos[2]
  if d <= 0.0:
    return
  fovD = d * math.tan(math.radians(fov) / 2.0)
  return (xyzLocalPos[0] + fovD) / (fovD * 2.0), (xyzLocalPos[1] + fovD) / (fovD * 2.0)


if __name__ == '__main__':
  print(W2L((math.pi * 0.25, 0, 0), (0, 0, 0), 60, (1, 1, 1)))
